//
// Created by LiuDongPeng on 2023/11/16.
//

#include "pid.h"


int pid_ctrl_init(pid_ctrl_t *pid, float p, float i, float d, float integralMax, float lowLimit, float upLimit)
{
    if (pid == 0)
        return -1;

    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->kb = 0.0f;
    pid->lowLimit = lowLimit;
    pid->upLimit = upLimit;

    pid->errorSumMax = 5.0f;
    pid->integralMax = integralMax;

    pid->ts = 1.0f / 20e3f;

    return 0;
}


/**
 * @brief Get current pi controller calc result
 * @param pid
 * @param tarIdq
 * @param fdkIdq
 * @param udq
 * @return
 */
float pid_ctrl_current_calc(pid_ctrl_t *pid, float tarIdq, float fdkIdq, float *udq)
{
    if (pid == 0)
        return 0;

    float output;
    float err = tarIdq - fdkIdq;

    /* 积分限幅 */
    if (true)
    {
        pid->integralSum += err;
        if (pid->integralSum > pid->integralMax)
            pid->integralSum = pid->integralMax;
        if (pid->integralSum < -pid->integralMax)
            pid->integralSum = -pid->integralMax;
    }

    /* 计算输出值 */
    output = (pid->kp * err) + (pid->ki * pid->integralSum);

    /* 输出限幅 */
    if (output > pid->upLimit)
        output = pid->upLimit;
    if (output < pid->lowLimit)
        output = pid->lowLimit;

    /* 误差传递 */
    pid->lastError = err;

    if (udq)
        *udq = output;

    pid->output = output;
    return output;
}


/**
 * @brief Get speed pi controller calc result
 * @param pid
 * @param tarSpeed
 * @param fdkSpeed
 * @param iqRef
 * @return
 */
float pid_ctrl_speed_calc(pid_ctrl_t *pid, float tarSpeed, float fdkSpeed, float *iqRef)
{
    if (pid == 0)
        return 0;

    float output;
    float err = tarSpeed - fdkSpeed;

    /* 积分限幅 */
    if (true)
    {
        pid->integralSum += err;
//        if (pid->integralSum > pid->integralMax)
//            pid->integralSum = pid->integralMax;
//        if (pid->integralSum < -pid->integralMax)
//            pid->integralSum = -pid->integralMax;
    }

    /* 计算输出值 */
    output = (pid->kp * err) + (pid->ki * pid->integralSum);
    /* 输出限幅 */
    if (output > pid->upLimit)
        output = pid->upLimit;
    if (output < pid->lowLimit)
        output = pid->lowLimit;

    /* 误差传递 */
    pid->lastError = err;

    if (iqRef)
        *iqRef = output;

    pid->output = output;
    return output;
}


float pid_ctrl_position_calc(pid_ctrl_t *pid, float tarPos, float fdkPos, float *speed)
{
    if (pid == 0)
        return 0;

    float output;
    float err = tarPos - fdkPos;

    /* 积分限幅 */
    if (true)
    {
        pid->integralSum += err;
        if (pid->integralSum > pid->integralMax)
            pid->integralSum = pid->integralMax;
        if (pid->integralSum < -pid->integralMax)
            pid->integralSum = -pid->integralMax;
    }

    /* 计算输出值 */
    output = (pid->kp * err) + (pid->ki * pid->integralSum);
    /* 输出限幅 */
    if (output > pid->upLimit)
        output = pid->upLimit;
    if (output < pid->lowLimit)
        output = pid->lowLimit;

    /* 误差传递 */
    pid->lastError = err;

    if (speed)
        *speed = output;

    pid->output = output;
    return output;
}
